/*************************************************************************\

Demo code for a simple sphere-physics world

\**************************************************************************/


#include "main.h"

// id needed for haptic shape.
HLuint gMyShapeId;

using namespace std;

// The collection of spheres
vector< sphere > spheres;

// The six walls. Spheres and planes are defines in objects.h
plane walls[6];
double wall;
// Hitting 's' adds energy to the scene with some scaling as defined below. 
double shakemag = 100.0;



int mode;
double beginx, beginy;

// Some stuff for allowing mouse rotations of the scene
double dis = 0.0, azim = 0.0, elev = 0.0;
double ddis = 0.0, dazim = 0.0, delev = 0.0;
double rot1 = 0.0, rot2 = 0.0, rot3 = 0.0;
int animate = 0;

// The timestep of the simulation
double deltat = 0.01;
double half_deltat = deltat / 2.0;
// Number of spheres in the sim
int numspheres = 10;
// Either do Euler or midpoint integration
bool euler = true;
// wall spring constant
double wall_spring = 100.0;
// ball spring constant
double ball_spring = 1000.0;
// Define gravity vector
double gravity[3] = {0.0, -9.8, 0.0};
// Define air friction scaling
double air_friction = 0.3;

void
InitViewerWindow()
{
	GLfloat Ambient[] = { 0.4, 0.4, 0.4, 1.0 };  
	GLfloat Diffuse[] = { 0.5, 0.5, 0.5, 1.0 };  
	GLfloat Specular[] = { 0.2, 0.2, 0.2, 1 };   
	GLfloat SpecularExp[] = { 50 };              
	GLfloat Emission[] = { 0.1, 0.1, 0.1, 1 };

	glMaterialfv(GL_FRONT, GL_AMBIENT, Ambient);
	glMaterialfv(GL_FRONT, GL_DIFFUSE, Diffuse);
	glMaterialfv(GL_FRONT, GL_SPECULAR, Specular);
	glMaterialfv(GL_FRONT, GL_SHININESS, SpecularExp);
	glMaterialfv(GL_FRONT, GL_EMISSION, Emission);

	glMaterialfv(GL_BACK, GL_AMBIENT, Ambient);
	glMaterialfv(GL_BACK, GL_DIFFUSE, Diffuse);
	glMaterialfv(GL_BACK, GL_SPECULAR, Specular);
	glMaterialfv(GL_BACK, GL_SHININESS, SpecularExp);
	glMaterialfv(GL_BACK, GL_EMISSION, Emission);

	glColorMaterial(GL_FRONT_AND_BACK, GL_DIFFUSE);

	glEnable(GL_COLOR_MATERIAL);

	GLfloat light_position[] = { 1.0, 1.0, 1.0, 0.0 };
	glLightfv(GL_LIGHT0, GL_POSITION, light_position);
	glEnable(GL_LIGHT0);
	GLfloat light_position2[] = { 1.0, 1.0, -1.0, 0.0 };
	glLightfv(GL_LIGHT1, GL_POSITION, light_position2);
	glEnable(GL_LIGHT1);
	glEnable(GL_LIGHTING);
	glLightModeli(GL_LIGHT_MODEL_TWO_SIDE, GL_TRUE);

	glDepthFunc(GL_LEQUAL);
	glEnable(GL_DEPTH_TEST);

	glShadeModel(GL_FLAT);
	glClearColor(0.0, 0.0, 0.0, 0.0);

	glEnable(GL_CULL_FACE);
	glCullFace(GL_BACK);
	glEnable(GL_NORMALIZE);

	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	glFrustum(-0.004,0.004,-0.004,0.004,.02,15.0);

	glMatrixMode(GL_MODELVIEW);
}

void
BeginDraw()
{
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	glLoadIdentity();  
	glTranslatef(0.0, 0.0, -(dis+ddis));
	glRotated(elev+delev, 1.0, 0.0, 0.0);
	glRotated(azim+dazim, 0.0, 1.0, 0.0);
}

void
EndDraw()
{
	glFlush();
	glutSwapBuffers();
}

void 
KeyboardCB(unsigned char key, int x, int y) 
{

	switch(key) 
	{
	case 'q': exit(0);
	case 's':
		for ( unsigned int i = 0; i < spheres.size(); i++ )
		{
			double shake[3];
			shake[0] = (rand() / (double)RAND_MAX) * 0.002 - 0.001;
			shake[1] = (rand() / (double)RAND_MAX) * 0.002 - 0.001;
			shake[2] = (rand() / (double)RAND_MAX) * 0.002 - 0.001;
			VpVxS( spheres[i].v, spheres[i].v, shake, shakemag );
		}
		shakemag *= 2.0;
		break;
	case 'e':
		euler = !euler;
		break;
	case '>':
		deltat = deltat * 2.0;
		break;
	case '<':
		deltat = deltat / 2.0;
		break;

	case '+':
		for ( int i = 0; i < 1; i++ )
		{
			sphere s;
			s.p[0] = 2.0 * wall * (rand() / (double)RAND_MAX) - wall;
			s.p[1] = 2.0 * wall * (rand() / (double)RAND_MAX) - wall;
			s.p[2] = 2.0 * wall * (rand() / (double)RAND_MAX) - wall;
			s.r = 0.1;

			s.v[0] = s.v[1] = s.v[2] = 0.0;
			s.mass = 0.1;
			//		s.mass = rand() / (double)RAND_MAX + 0.2;
			spheres.push_back( s );
		}
		numspheres +=5;
		break;

	case '-':
		numspheres -=1;		
		spheres.resize( numspheres );

		break;

	default: animate = 1 - animate;
	}

	glutPostRedisplay();
}

void
MouseCB(int _b, int _s, int _x, int _y)
{
	if (_s == GLUT_UP)
	{
		dis += ddis;
		azim += dazim;
		elev += delev;
		ddis = 0.0;
		dazim = 0.0;
		delev = 0.0;
		return;
	}

	if (_b == GLUT_RIGHT_BUTTON)
	{
		mode = 0;
		beginy = _y;
		return;
	}
	else
	{
		mode = 1;
		beginx = _x;
		beginy = _y;
	}
}

void
MotionCB(int _x, int _y)
{
	if (mode == 0)
	{
		ddis = dis * (_y - beginy)/200.0;
	}
	else
	{
		dazim = (_x - beginx)/5.0;
		delev = (_y - beginy)/5.0;      
	}

	glutPostRedisplay();
}

void computeForce( unsigned int i, double force[3] )
{

	// Non-colliding case:
	//    force:
	//			gravity - only in the -y direction, F = mg
	//		    air friction - proportional to sphere speed F = -va

	// Compute non-colliding forces
	for ( int d = 0; d < 3; d++ ) {
		force[d] = spheres[i].mass * gravity[d] - spheres[i].v[d] * air_friction;
		//add in the haptic force
		force[d] += spheres[i].haptic_force[d];
		spheres[i].haptic_force[d] = 0;
	}

	
	// Now check for collisions with the other spheres
	for ( unsigned int j = 0; j < spheres.size(); j++ )
	{
		// Don't collide with yourself
		if ( j != i )
		{	
			double dist = spheres[i].distSphereSphere(spheres[j] );

			// If colliding, bounce apart with a penalty spring force proportional to penetration
			if ( dist <  0.0 )
			{
				// f is the magnitude of the force
				double f = dist * ball_spring;
				// N is the direction of the force
				double N[3];
				VmV( N, spheres[i].p, spheres[j].p );
				Vnormalize( N );
				VxS( N, N, -f );
				VpV( force, force, N );
			}
		}
	}
	// Now check for collisions with the box walls
	for ( int j = 0; j < 6; j++ )
	{
		double dist = spheres[i].distSpherePlane(walls[j] );

		if ( dist <  0.0 )
		{
			double f = -dist * wall_spring;
			VpVxS( force, force, walls[j].N, f); 
		}
	}
}

void
IdleCB() 
{
	// I wanted to let the user do more and more violent shaking. So the shaking decays
	// over time, but also doubles in magnitude when the scene is shook.
	shakemag = shakemag * 0.99;
	if ( shakemag < 10.0 )
		shakemag = 10.0;

	// HEY - this will crash if you set numspheres >= 1000
	double forces[1000][3];

	// Do the physics simulation
	for ( unsigned int i = 0; i < spheres.size(); i++ )
	{
		computeForce( i, forces[i] );
	}

	if ( euler )
	{
		// Do the Euler integration
		for ( unsigned int i = 0; i < spheres.size(); i++ )
		{	
			//printf("Sphere %d:\tforce: %f\tvelocity: %f\tposition: %f\n", i, forces[i][0], spheres[i].v[0], spheres[i].p[0]);
			for(int j = 0; j < 3; j++) {
				//change the position first
				spheres[i].p[j] = spheres[i].p[j] + spheres[i].v[j] * deltat;
				//now fix the velocity
				spheres[i].v[j] = spheres[i].v[j] + (forces[i][j] / spheres[i].mass) * deltat;
				//try clearing the force now
				forces[i][j] = 0;
			}
		}
	}
	else // Do midpoint eval
	{
		for ( unsigned int i = 0; i < spheres.size(); i++ )
		{
			// Move forward half
			double old_x = spheres[i].p[0];
			double old_y = spheres[i].p[1];
			double old_z = spheres[i].p[2];

			for(int j = 0; j < 3; j++) {
				spheres[i].p[j] = spheres[i].p[j] + spheres[i].v[j] * half_deltat;
			}
			// Do the physics simulation at the midway point
			for(int j = 0; j < 3; j++) {
				spheres[i].v[j] = spheres[i].v[j] + (forces[i][j] / spheres[i].mass) * half_deltat;
			}
			// Move back to the start
			spheres[i].p[0] = old_x;
			spheres[i].p[1] = old_y;
			spheres[i].p[2] = old_z;

			// Now move forward again
			for(int j = 0; j < 3; j++) {
				spheres[i].p[j] = spheres[i].p[j] + spheres[i].v[j] * half_deltat;
			}
		}
	
	}
	glutPostRedisplay();


}


void
DisplayCB()
{
	BeginDraw();

	DrawSpheres();
	
    // Draw a sphere using OpenGL.
    glutSolidSphere(0.5, 32, 32);

	EndDraw();
	
}

void DrawSpheres()
{
	for ( unsigned int i = 0; i < spheres.size(); i++ )
		spheres[i].draw();
}

void DrawWalls()
{
	walls[0].draw(wall);
	walls[1].draw(wall);
	walls[2].draw(wall);
	walls[3].draw(wall);
	walls[4].draw(wall);
	walls[5].draw(wall);
}

void init_walls(void)
{
	wall = 0.9;
	for ( int i = 0; i < numspheres; i++ )
	{
		sphere s;
		s.p[0] = 2.0 * wall * (rand() / (double)RAND_MAX) - wall;
		s.p[1] = 2.0 * wall * (rand() / (double)RAND_MAX) - wall;
		s.p[2] = 2.0 * wall * (rand() / (double)RAND_MAX) - wall;
		s.r = 0.1;

		s.v[0] = s.v[1] = s.v[2] = 0.0;
		s.mass = 0.1;
//		s.mass = rand() / (double)RAND_MAX + 0.2;
		spheres.push_back( s );
	}

	wall = 1.0;


	walls[0].N[1] = 1.0;
	walls[0].p[1] = -wall;

	walls[1].N[0] = 1.0;
	walls[1].p[0] = -wall;

	walls[2].N[0] = -1.0;
	walls[2].p[0] = wall;

	walls[3].N[2] = 1.0;
	walls[3].p[2] = -wall;

	walls[4].N[2] = -1.0;
	walls[4].p[2] = wall;

	walls[5].N[1] = -1.0;
	walls[5].p[1] = wall;
}

void init(int argc, char **argv)
{
   glutInit(&argc, argv);
       glutInitDisplayMode (GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH | GLUT_MULTISAMPLE);

       // create the window

       glutInitWindowSize( 700, 700 );
       glutCreateWindow("SphereWorld");

       // set OpenGL graphics state -- material props, perspective, etc.

       InitViewerWindow();

       // set the callbacks

       glutDisplayFunc(DisplayCB);
	   //glutDisplayFunc(glutDisplay);
       glutIdleFunc(IdleCB);
       glutMouseFunc(MouseCB);
       glutMotionFunc(MotionCB);  
       glutKeyboardFunc(KeyboardCB);

	   init_walls();
}

/*
int main(int argc, char **argv)
{
       
       init(argc, argv);
       glutMainLoop();
}*/
